/*!
 * @file YFRobot_VL53L0X.cpp
 * @n This example provides the VL53L0X laser rangefinder API function
 */

#include "YFRobot_VL53L0X.h"
VL53L0X_DetailedData_t DetailedData;

YFRobot_VL53L0X::YFRobot_VL53L0X()
{
	i2cFirst = true;
	singleReadFrist = true;
}

YFRobot_VL53L0X::~YFRobot_VL53L0X()
{
}

void YFRobot_VL53L0X::begin(uint8_t i2c_addr)
{
	uint8_t val1;
	if (i2cFirst)
	{
		Wire.begin();
		i2cFirst = false;
	}
	delay(1500);
	DetailedData.I2cDevAddr = I2C_DevAddr;
	DataInit();
	setDeviceAddress(i2c_addr);
	val1 = readByteData(VL53L0X_REG_IDENTIFICATION_REVISION_ID);
	//Serial.println("");
	//Serial.print("Revision ID: "); Serial.println(val1,HEX);

	val1 = readByteData(VL53L0X_REG_IDENTIFICATION_MODEL_ID);
	//Serial.print("Device ID: "); Serial.println(val1,HEX);
	//Serial.println("");
}

void YFRobot_VL53L0X::DataInit()
{
	uint8_t data;
#ifdef ESD_2V8
	data = readByteData(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV);
	data = (data & 0xFE) | 0x01;
	writeByteData(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, data);
#endif
	writeByteData(0x88, 0x00);
	writeByteData(0x80, 0x01);
	writeByteData(0xFF, 0x01);
	writeByteData(0x00, 0x00);
	readByteData(0x91);
	writeByteData(0x91, 0x3c);
	writeByteData(0x00, 0x01);
	writeByteData(0xFF, 0x00);
	writeByteData(0x80, 0x00);
}

void YFRobot_VL53L0X::writeData(unsigned char Reg, unsigned char *buf,
								unsigned char Num)
{
	for (unsigned char i = 0; i < Num; i++)
	{
		writeByteData(Reg++, buf[i]);
	}
}

void YFRobot_VL53L0X::writeByteData(unsigned char Reg, unsigned char byte)
{
	Wire.beginTransmission(DetailedData.I2cDevAddr); // transmit to device #8
	Wire.write(Reg);								 // sends one byte
	Wire.write((uint8_t)byte);
	Wire.endTransmission(); // stop transmitting
}

void YFRobot_VL53L0X::readData(unsigned char Reg, unsigned char Num)
{
	Wire.beginTransmission(DetailedData.I2cDevAddr); // transmit to device #8
	Wire.write((uint8_t)Reg);						 // sends one byte
	Wire.endTransmission();							 // stop transmitting
	Wire.requestFrom((uint8_t)DetailedData.I2cDevAddr, (uint8_t)Num);

	for (int i = 0; i < Num; i++)
	{
		DetailedData.originalData[i] = Wire.read();
		delay(1);
	}
}

uint8_t YFRobot_VL53L0X::readByteData(unsigned char Reg)
{
	uint8_t data;
	Wire.beginTransmission(DetailedData.I2cDevAddr); // transmit to device #8
	Wire.write((uint8_t)Reg);						 // sends one byte
	Wire.endTransmission();							 // stop transmitting
	Wire.requestFrom((uint8_t)DetailedData.I2cDevAddr, (uint8_t)1);
	data = Wire.read();
	return data;
}

void YFRobot_VL53L0X::start()
{
	uint8_t DeviceMode;
	uint8_t Byte;
	uint8_t StartStopByte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
	uint32_t LoopNb;

	DeviceMode = DetailedData.mode;

	writeByteData(0x80, 0x01);
	writeByteData(0xFF, 0x01);
	writeByteData(0x00, 0x00);
	writeByteData(0x91, 0x3c);
	writeByteData(0x00, 0x01);
	writeByteData(0xFF, 0x00);
	writeByteData(0x80, 0x00);

	switch (DeviceMode)
	{
	case VL53L0X_DEVICEMODE_SINGLE_RANGING:
		writeByteData(VL53L0X_REG_SYSRANGE_START, 0x01);
		Byte = StartStopByte;
		/* Wait until start bit has been cleared */
		LoopNb = 0;
		do
		{
			if (LoopNb > 0)
				Byte = readByteData(VL53L0X_REG_SYSRANGE_START);
			LoopNb = LoopNb + 1;
		} while (((Byte & StartStopByte) == StartStopByte) &&
				 (LoopNb < VL53L0X_DEFAULT_MAX_LOOP));
		break;
	case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
		/* Back-to-back mode */
		/* Check if need to apply interrupt settings */
		//VL53L0X_CheckAndLoadInterruptSettings(Dev, 1);中断检查?
		writeByteData(VL53L0X_REG_SYSRANGE_START, VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
		break;
	default:
		/* Selected mode not supported */
		//Serial.println("---Selected mode not supported---");
		break;
	}
}

void YFRobot_VL53L0X::readVL53L0X()
{
	readData(VL53L0X_REG_RESULT_RANGE_STATUS, 12);
	DetailedData.ambientCount = ((DetailedData.originalData[6] & 0xFF) << 8) |
								(DetailedData.originalData[7] & 0xFF);
	DetailedData.signalCount = ((DetailedData.originalData[8] & 0xFF) << 8) |
							   (DetailedData.originalData[9] & 0xFF);
	DetailedData.distance = ((DetailedData.originalData[10] & 0xFF) << 8) |
							(DetailedData.originalData[11] & 0xFF);
	DetailedData.status = ((DetailedData.originalData[0] & 0x78) >> 3);
}

void YFRobot_VL53L0X::setDeviceAddress(uint8_t newAddr)
{
	newAddr &= 0x7F;
	writeByteData(VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, newAddr);
	DetailedData.I2cDevAddr = newAddr;
}

void YFRobot_VL53L0X::highPrecisionEnable(FunctionalState NewState)
{
	writeByteData(VL53L0X_REG_SYSTEM_RANGE_CONFIG,
				  NewState);
}

void YFRobot_VL53L0X::setMode(PrecisionState precision, ModeState mode)
{
	DetailedData.mode = mode;
	if (precision == High)
	{
		highPrecisionEnable(ENABLE);
		DetailedData.precision = precision;
	}
	else
	{
		highPrecisionEnable(DISABLE);
		DetailedData.precision = precision;
	}
	this->start();
}

void YFRobot_VL53L0X::stop()
{
	writeByteData(VL53L0X_REG_SYSRANGE_START, VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);

	writeByteData(0xFF, 0x01);
	writeByteData(0x00, 0x00);
	writeByteData(0x91, 0x00);
	writeByteData(0x00, 0x01);
	writeByteData(0xFF, 0x00);
}

float YFRobot_VL53L0X::getDistance()
{
	if (DetailedData.mode == VL53L0X_DEVICEMODE_SINGLE_RANGING && singleReadFrist)
	{
		for (int i = 0; i < 5; i++)
		{
			readVL53L0X();
		}
		singleReadFrist = false;
	}
	readVL53L0X();
	if (DetailedData.distance == 20)
		DetailedData.distance = _distance;
	else
		_distance = DetailedData.distance;
	if (DetailedData.precision == High)
		return DetailedData.distance / 4.0 - 20.0;
	else
		return DetailedData.distance - 20.0;
}
/*
void YFRobot_VL53L0X::setOffsetCalibration(float OffsetCalibrationDataMicroMeter){
    _offset = OffsetCalibrationDataMicroMeter;
}

void YFRobot_VL53L0X::setOffsetCalibration(int32_t OffsetCalibrationDataMicroMeter)
{
	int32_t cMaxOffsetMicroMeter = 511000;
	int32_t cMinOffsetMicroMeter = -512000;
	int16_t cOffsetRange = 4096;
	uint32_t encodedOffsetVal;

	if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter)
		OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter;
	else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter)
		OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter;

	if (OffsetCalibrationDataMicroMeter >= 0) {
		encodedOffsetVal = OffsetCalibrationDataMicroMeter/250.0;
	} else {
		encodedOffsetVal = cOffsetRange + OffsetCalibrationDataMicroMeter/250.0;
	}
    Serial.println(encodedOffsetVal);
	writeByteData(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encodedOffsetVal);
}

int32_t YFRobot_VL53L0X::getOffsetCalibration()
{
	uint16_t RangeOffsetRegister;
	int16_t cMaxOffset = 2047;
	int16_t cOffsetRange = 4096;
    int32_t pOffsetCalibrationDataMicroMeter;


	RangeOffsetRegister = readByteData(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM);
    Serial.println(RangeOffsetRegister);
	//RangeOffsetRegister = (RangeOffsetRegister & 0x0fff);

	if (RangeOffsetRegister > cMaxOffset)
		pOffsetCalibrationDataMicroMeter = (int16_t)(RangeOffsetRegister - cOffsetRange) * 250;
	else
		pOffsetCalibrationDataMicroMeter = (int16_t)RangeOffsetRegister * 250;
    return pOffsetCalibrationDataMicroMeter;
}

*/

uint16_t YFRobot_VL53L0X::getAmbientCount()
{
	readVL53L0X();
	return DetailedData.ambientCount;
}

uint16_t YFRobot_VL53L0X::getSignalCount()
{
	readVL53L0X();
	return DetailedData.signalCount;
}

uint8_t YFRobot_VL53L0X::getStatus()
{
	readVL53L0X();
	return DetailedData.status;
}

void YFRobot_VL53L0X::setTime(int time)
{
	int i;
	for (i = 0; i < time; i++)
	{
		//Serial.print("Distance: ");Serial.println(this->getDistance());
	}
	//The delay is added to demonstrate the effect, and if you do not add the delay,
	//it will not affect the measurement accuracy
	delay(500);
}
